#include "stdafx.h"
#include "OpenCLManager.h"
#define SUCCESS 0
#define FAILURE 1


using namespace std;
using namespace appsdk;

int OpenCLManager::setupOCL()
{
	cl_int status = CL_SUCCESS;
    cl_device_type dType = CL_DEVICE_TYPE_GPU;

    /*
     * Have a look at the available platforms and pick either
     * the AMD one if available or a reasonable default.
     */
    platform = NULL;
    int retValue = getPlatform(platform, 0,false);
    CHECK_ERROR(retValue, SDK_SUCCESS, "getPlatform() failed");

    // Display available devices.
    retValue = displayDevices(platform, dType);
    CHECK_ERROR(retValue, SDK_SUCCESS, "displayDevices() failed");

    // If we could find our platform, use it. Otherwise use just available platform.
    cl_context_properties cps[] =
    {
		CL_GL_CONTEXT_KHR , (cl_context_properties) wglGetCurrentContext() ,
		CL_WGL_HDC_KHR , (cl_context_properties) wglGetCurrentDC() ,
        CL_CONTEXT_PLATFORM,(cl_context_properties)platform,
        0
    };

    context = clCreateContextFromType(cps, dType,NULL,NULL,&status);
    CHECK_OPENCL_ERROR(status, "clCreateContextFromType failed.");

    // getting device on which to run the sample
    status = getDevices(context, &devices, 0,false);
    CHECK_ERROR(status, SDK_SUCCESS, "getDevices() failed");
	device = devices[0];

    status = deviceInfo.setDeviceInfo(device);
    CHECK_OPENCL_ERROR(status, "deviceInfo.setDeviceInfo failed");

    //if(!deviceInfo.imageSupport){
    //    OPENCL_EXPECTED_ERROR(" Expected Error: Device does not support Images");
    //}

    // Create command queue
    cl_command_queue_properties prop = 0;
    commandQueue = clCreateCommandQueue(
                       context,
                       device,
                       prop,
                       &status);
    CHECK_OPENCL_ERROR(status,"clCreateCommandQueue failed.");

	// initialize the Image data to NULL
	outputImageData2D = new float[width*height];

    /* === BUILD PROGRAM 1 === */
    buildProgramData buildData;
    buildData.kernelName = std::string(OPENCL_FILE);
    buildData.devices = devices;
    buildData.deviceId = sampleArgs->deviceId;
    buildData.flagsStr = std::string("");

    retValue = buildOpenCLProgram(program, context, buildData);
    CHECK_ERROR(retValue, SDK_SUCCESS, "buildOpenCLProgram() failed");

#ifdef ALGORITHMS_SIMULATE
	mKernel_MatchingDepth = clCreateKernel(program, OPENCL_KERNEL_SIMULATE_PSO, &status);
#else
	mKernel_MatchingDepth = clCreateKernel(program, OPENCL_KERNEL_PSO, &status);
#endif // ALGORITHMS_SIMULATE

    
	mKernelReduction = clCreateKernel(program, OPENCL_KERNEL_REDUCTION, &status);
	
    CHECK_OPENCL_ERROR(status,"clCreateKernel failed.(kernel2D)");
	/* === END BUILD PROGRAM 1 === */

    // Check group size against group size returned by kernel
    status = clGetKernelWorkGroupInfo(mKernel_MatchingDepth,
                                      device,
                                      CL_KERNEL_WORK_GROUP_SIZE,
                                      sizeof(size_t),
                                      &kernel2DWorkGroupSize,
                                      0);
    CHECK_OPENCL_ERROR(status,"clGetKernelWorkGroupInfo  failed.");

    return SDK_SUCCESS;
}

int OpenCLManager::runCLKernel_GetDforSwarm_Pixels(std::vector<double> &measureResult)
{
	size_t local_size = kernel2DWorkGroupSize;
	size_t num_particles = measureResult.size();
	size_t global_size = width*height*num_particles; 
	glFinish();
	status = clEnqueueAcquireGLObjects(commandQueue,1,&renderedDepthCL,0,0,NULL);
	CHECK_OPENCL_ERROR(status,"clAcquire Render failed.");
	//showDataCLMem1D(128,128*num_particles,renderedDepthCL,commandQueue,"REN");
	
	cl_mem sum_1_cl = clCreateBuffer(context, CL_MEM_READ_ONLY , (global_size) * sizeof(ushort), NULL, NULL);
	cl_mem sum_2_cl = clCreateBuffer(context, CL_MEM_READ_ONLY , (global_size) * sizeof(ushort), NULL, NULL);
	cl_mem sum_3_cl = clCreateBuffer(context, CL_MEM_READ_ONLY , (global_size) * sizeof(ushort), NULL, NULL);
	
	/* Matching Depth */
	status = clSetKernelArg(mKernel_MatchingDepth, 5, sizeof(cl_mem), &sum_1_cl);CHECK_OPENCL_ERROR(status,"clSetKernelArg failed.");
	status = clSetKernelArg(mKernel_MatchingDepth, 6, sizeof(cl_mem), &sum_2_cl);CHECK_OPENCL_ERROR(status,"clSetKernelArg failed.");
	status = clSetKernelArg(mKernel_MatchingDepth, 7, sizeof(cl_mem), &sum_3_cl);CHECK_OPENCL_ERROR(status,"clSetKernelArg failed.");

	cl_event matchingEvt;
	status = clEnqueueNDRangeKernel(commandQueue, mKernel_MatchingDepth, 1, NULL, &global_size, NULL, 0, NULL, &matchingEvt);
	CHECK_OPENCL_ERROR(status,"clEnqueueNDRangeKernel failed.");

	status = clFlush(commandQueue);
	CHECK_OPENCL_ERROR(status, "clFlush failed.");
	//showMemSum1D(128,128*num_particles,sum_1_cl,commandQueue,"S1");
	//cv::waitKey();
	//showMemSum1D(128,128*num_particles,sum_2_cl,commandQueue,"S1");
	//cv::waitKey();
	//showMemSum1D(128,128*num_particles,sum_3_cl,commandQueue,"S1");
	//cv::waitKey();
	status = waitForEventAndRelease(&matchingEvt);
	
	CHECK_ERROR(status, SDK_SUCCESS, "WaitForEventAndRelease(writeEvt) Failed");

	std::vector<double> sum1_array(num_particles),sum2_array(num_particles),sum3_array(num_particles);
	calculateSumCL(sum_1_cl,global_size,sum1_array);
	calculateSumCL(sum_2_cl,global_size,sum2_array);
	calculateSumCL(sum_3_cl,global_size,sum3_array);

	for (int i=0;i<num_particles;i++){
		measureResult[i] = sum1_array[i]/(sum2_array[i]) + 20*(1 - 2*sum3_array[i]/(sum2_array[i]+sum3_array[i]));
	}

	clReleaseMemObject(sum_1_cl);
	clReleaseMemObject(sum_2_cl);
	clReleaseMemObject(sum_3_cl);
	
	clEnqueueReleaseGLObjects(commandQueue, 1, &renderedDepthCL, 0, NULL, NULL);
	clFinish(commandQueue);
	return SDK_SUCCESS;
}

int OpenCLManager::calculateSumCL(cl_mem inputMem,size_t arraySize,std::vector<double>&sum)
{
	/* REDUCTION-VECTOR 8 ushort*/
	size_t num_groups = (size_t)(arraySize/kernel2DWorkGroupSize);
	size_t num_result = sum.size();
	cl_mem outputMem = clCreateBuffer(context, CL_MEM_READ_WRITE , num_groups * sizeof(cl_ushort), NULL, NULL);

	cl_ushort * outData = new cl_ushort[num_groups/8];
	size_t global_size = arraySize/8;
	size_t local_size = kernel2DWorkGroupSize;

	status = clSetKernelArg(mKernelReduction, 0, sizeof(cl_mem), &inputMem);CHECK_OPENCL_ERROR(status,"clSetKernelArg failed.");
	status = clSetKernelArg(mKernelReduction, 1, 8 * kernel2DWorkGroupSize * sizeof(cl_ushort), NULL);CHECK_OPENCL_ERROR(status,"clSetKernelArg failed.");
	status = clSetKernelArg(mKernelReduction, 2, sizeof(cl_mem), &outputMem);CHECK_OPENCL_ERROR(status,"clSetKernelArg failed.");

	status = clEnqueueNDRangeKernel(commandQueue, mKernelReduction, 1, NULL, &global_size, &local_size, 0, NULL, NULL);CHECK_OPENCL_ERROR(status,"clEnqueueNDRangeKernel failed.");

	status = clEnqueueReadBuffer(commandQueue, outputMem, CL_TRUE, 0, num_groups/8 * sizeof(cl_ushort), outData, 0, NULL, NULL);

	status = clFlush(commandQueue);CHECK_OPENCL_ERROR(status, "clFlush failed.");

	size_t particle_size_reducted = 8;// before 128*128 after devided for 2048
	for (int i=0; i<num_result; i++)
	{
		size_t globalOffset = i*particle_size_reducted;
		for (int j=0; j<particle_size_reducted; j++)
		{
			sum[i]+= outData[globalOffset+j];
		}
	}

	clReleaseMemObject(outputMem);
	FREE(outData);
	return SDK_SUCCESS;
}

float OpenCLManager::reductionNoneCLKernel(cl_mem inputMem,size_t arraySize)
{
	float * outData = new float[arraySize];
	status = clEnqueueReadBuffer(commandQueue, inputMem, CL_TRUE, 0, arraySize*sizeof(float), outData, 0, NULL, NULL);
	status = clFlush(commandQueue);CHECK_OPENCL_ERROR(status, "clFlush failed.");
	float sum = 0;

	for (int i=0;i<arraySize;i++){
		sum+=outData[i];
	}
	FREE(outData);

	return sum;
}

int OpenCLManager::setupMatchingKernel(GLuint renderBuf)
{
	size_t global_size = width*height*NUM_TOTAL_PARTICLES; 

	observeDepthCL = clCreateBuffer(context, CL_MEM_READ_WRITE, sizeof(cl_ushort)*global_size, NULL, &status);
	CHECK_OPENCL_ERROR(status,"clCreateBuffer observeDepthCL failed.");
	observeSkinCL = clCreateBuffer(context,CL_MEM_READ_WRITE, global_size, NULL, &status);
	CHECK_OPENCL_ERROR(status,"clCreateBuffer observeSkinCL failed.");

	renderedDepthCL = clCreateFromGLBuffer(context, CL_MEM_READ_ONLY, renderBuf, &status);
	CHECK_OPENCL_ERROR(status,"clCreateBuffer renderedDepthCL failed.");

	status = clSetKernelArg(mKernel_MatchingDepth, 0, sizeof(cl_mem), &observeDepthCL);CHECK_OPENCL_ERROR(status,"clSetKernelArg failed.");
	status = clSetKernelArg(mKernel_MatchingDepth, 1, sizeof(cl_mem), &renderedDepthCL);CHECK_OPENCL_ERROR(status,"clSetKernelArg failed.");
	status = clSetKernelArg(mKernel_MatchingDepth, 2, sizeof(cl_mem), &observeSkinCL);CHECK_OPENCL_ERROR(status,"clSetKernelArg failed.");

	return SDK_SUCCESS;
}
int OpenCLManager::setObserve(cl_ushort * depthBuf, uchar * skinBuf, cl_ushort depthScale)
{
	size_t p_size = width*height;
	for (int i=0;i<NUM_TOTAL_PARTICLES;i++){
		clEnqueueWriteBuffer(commandQueue, observeDepthCL, CL_TRUE,
			i*p_size*sizeof(cl_ushort), p_size*sizeof(cl_ushort), (void*)depthBuf, 0, NULL, NULL);
	}
	for (int i=0;i<NUM_TOTAL_PARTICLES;i++){
		clEnqueueWriteBuffer(commandQueue, observeSkinCL, CL_TRUE,
			i*p_size, p_size, (void*)skinBuf, 0, NULL, NULL);
	}
	cl_ushort highThresh = DMAX*depthScale;
	cl_ushort lowThresh = Dmax*depthScale;
	status = clSetKernelArg(mKernel_MatchingDepth, 3, sizeof(cl_ushort),&highThresh);
	status = clSetKernelArg(mKernel_MatchingDepth, 4, sizeof(cl_ushort),&lowThresh);
	//showDataCLMem1D(128,128*64,observeDepthCL,commandQueue,"Obs");
	//showCharDataCLMem1D(128,128*64,observeSkinCL,commandQueue,"Skin");
	return SDK_SUCCESS;
}

int OpenCLManager::setupSimulatingKernel(GLuint depthBuf, GLuint skinBuf, GLuint renderBuf)
{
	observeDepthCL = clCreateFromGLBuffer(context, CL_MEM_READ_ONLY, depthBuf, &status);
	CHECK_OPENCL_ERROR(status,"clCreateBuffer observeDepthCL failed.");
	observeSkinCL = clCreateFromGLBuffer(context, CL_MEM_READ_ONLY, skinBuf, &status);
	CHECK_OPENCL_ERROR(status,"clCreateBuffer observeSkinCL failed.");
	renderedDepthCL = clCreateFromGLBuffer(context, CL_MEM_READ_ONLY, renderBuf, &status);
	CHECK_OPENCL_ERROR(status,"clCreateBuffer renderedDepthCL failed.");

	float depthS_to_mm = 1000.f/0xffff;
	cl_ushort Hmax = cl_ushort(DMAX/depthS_to_mm);
	cl_ushort Hmin = cl_ushort(Dmax/depthS_to_mm);

	status = clSetKernelArg(mKernel_MatchingDepth, 0, sizeof(cl_mem), &observeDepthCL);CHECK_OPENCL_ERROR(status,"clSetKernelArg failed.");
	status = clSetKernelArg(mKernel_MatchingDepth, 1, sizeof(cl_mem), &renderedDepthCL);CHECK_OPENCL_ERROR(status,"clSetKernelArg failed.");
	status = clSetKernelArg(mKernel_MatchingDepth, 2, sizeof(cl_mem), &observeSkinCL);CHECK_OPENCL_ERROR(status,"clSetKernelArg failed.");
	status = clSetKernelArg(mKernel_MatchingDepth, 3, sizeof(cl_ushort),&Hmax);
	status = clSetKernelArg(mKernel_MatchingDepth, 4, sizeof(cl_ushort),&Hmin);
	return SDK_SUCCESS;
}

int OpenCLManager::acquireObservePixel()
{
	glFinish();	
	status  = clEnqueueAcquireGLObjects(commandQueue,1,&observeDepthCL,0,0,NULL);
	status |= clEnqueueAcquireGLObjects(commandQueue,1,&observeSkinCL,0,0,NULL);
	CHECK_OPENCL_ERROR(status,"clAcquire Observe failed.");
	return SDK_SUCCESS;
}